#include "WPIlib.h"
#include "RMRDrive.h"
#include "RMRMain.h"
#include "RMRUtil.h"
#include "RMRPIDIO.h"
#include "RMRAccel.h"
#include "AnalogChannel.h"
#include "math.h"

extern "C"
{
	extern bool IS_CAL;
};


/*
 * RMRDrive.cpp
 * 
 * Definition of class RMRDrive,
 * which handles the driving aspect of the robot
 * 
 * Most important function: CarDrive() -- call this from the main robot loop
 */


// constructor
RMRDrive::RMRDrive(Watchdog &watchdog):
	// initialize members...
	// MUST BE IN SAME ORDER AS RMRDrive class declaration
	steer_abs_encoder(	SIDECAR_1, STEER_ENCODER),
	steer_motor(		SIDECAR_1, STEER_MOTOR_CHANNEL),
	
	left_front_motor(	SIDECAR_1, LEFT_FRONT_MOTOR_CHANNEL),
	right_front_motor(	SIDECAR_1, RIGHT_FRONT_MOTOR_CHANNEL),
	left_back_motor(	SIDECAR_1, LEFT_BACK_MOTOR_CHANNEL),
	right_back_motor(	SIDECAR_1, RIGHT_BACK_MOTOR_CHANNEL),
	
	left_front_encoder(	SIDECAR_2, LEFT_FRONT_ENCODER_A,
						SIDECAR_2, LEFT_FRONT_ENCODER_B,
						LEFT_FRONT_ENCODER_REVERSED, 
						FRONT_LEFT_ENCODER_TYPE),
	right_front_encoder(SIDECAR_2, RIGHT_FRONT_ENCODER_A,
				  		SIDECAR_2, RIGHT_FRONT_ENCODER_B,
				  		RIGHT_FRONT_ENCODER_REVERSED,
				  		FRONT_RIGHT_ENCODER_TYPE),
	left_back_encoder(	SIDECAR_2, LEFT_BACK_ENCODER_A,
				 		SIDECAR_2, LEFT_BACK_ENCODER_B,
				 		LEFT_BACK_ENCODER_REVERSED,
				 		BACK_LEFT_ENCODER_TYPE),
	right_back_encoder(	SIDECAR_2, RIGHT_BACK_ENCODER_A,
				  		SIDECAR_2, RIGHT_BACK_ENCODER_B,
				  		RIGHT_BACK_ENCODER_REVERSED,
				  		BACK_RIGHT_ENCODER_TYPE),
				  		
	left_front_pid(	FRONT_PID_K_P,	FRONT_PID_K_I,	FRONT_PID_K_D,	&left_front_encoder,	&left_front_motor),
	right_front_pid(FRONT_PID_K_P,	FRONT_PID_K_I,	FRONT_PID_K_D,	&right_front_encoder,	&right_front_motor),
	left_back_pid(	BACK_PID_K_P,	BACK_PID_K_I,	BACK_PID_K_D,	&left_back_encoder,		&left_back_motor),
	right_back_pid(	BACK_PID_K_P,	BACK_PID_K_I,	BACK_PID_K_D,	&right_back_encoder,	&right_back_motor),
	steer_pid(		STEER_PID_K_P,	STEER_PID_K_I,	STEER_PID_K_D,	&steer_abs_encoder,		&steer_motor),
	
	acc_x(new AnalogChannel(ANALOG, ACCEL_X)),
	acc_y(new AnalogChannel(ANALOG, ACCEL_Y)),
	
	m_watchdog(watchdog),
	
	m_left_limit_switch(	SIDECAR_1, STEER_LIMIT_LEFT),
	m_right_limit_switch(	SIDECAR_1, STEER_LIMIT_RIGHT),
	
	steering_wheel(	STEERING_WHEEL_CHANNEL),
	joystick2(		JOYSTICK2_CHANNEL)
{

	// initialize encoders
	left_front_encoder.SetScaling(	FRONT_ENCODER_SCALING);
	right_front_encoder.SetScaling(	FRONT_ENCODER_SCALING);
	left_back_encoder.SetScaling(	BACK_ENCODER_SCALING);
	right_back_encoder.SetScaling(	BACK_ENCODER_SCALING);

	left_front_encoder.SetSpeedMode();
	right_front_encoder.SetSpeedMode();
	left_back_encoder.SetSpeedMode();
	right_back_encoder.SetSpeedMode();
	
	left_front_encoder.Start();
	right_front_encoder.Start();
	left_back_encoder.Start();
	right_back_encoder.Start();
	
	// init steering encoder (these values are almost guaranteed to not work - need to call AutoCalibrate)
	steer_abs_encoder.SetMin(ANGLE_MIN);
	steer_abs_encoder.SetMax(ANGLE_MAX);
	steer_abs_encoder.SetBackwards(ANGLE_IS_BACKWARDS);
	
	// initialize motor direction
	left_front_motor.SetInverted(	LEFT_FRONT_MOTOR_INVERTED);
	right_front_motor.SetInverted(	RIGHT_FRONT_MOTOR_INVERTED);
	left_back_motor.SetInverted(	LEFT_BACK_MOTOR_INVERTED);
	right_back_motor.SetInverted(	RIGHT_BACK_MOTOR_INVERTED);
	
	steer_motor.SetInverted(STEER_MOTOR_INVERTED);
	
	// temporary debug stuff
	printf("motors reversed: %s, %s, %s, %s\n",
							LEFT_FRONT_MOTOR_INVERTED ? "t" : "f",
							RIGHT_FRONT_MOTOR_INVERTED ? "t" : "f",
							LEFT_BACK_MOTOR_INVERTED ? "t" : "f",
							RIGHT_BACK_MOTOR_INVERTED ? "t" : "f");
	
	// initialize PID
	left_front_pid.SetInputRange(MIN_SPEED, MAX_SPEED);
	left_front_pid.SetOutputRange(-1.0, 1.0);
	
	right_front_pid.SetInputRange(MIN_SPEED, MAX_SPEED);
	right_front_pid.SetOutputRange(-1.0, 1.0);
	
	left_back_pid.SetInputRange(MIN_SPEED, MAX_SPEED);
	left_back_pid.SetOutputRange(-1.0, 1.0);
	
	right_back_pid.SetInputRange(MIN_SPEED, MAX_SPEED);
	right_back_pid.SetOutputRange(-1.0, 1.0);

	steer_pid.SetInputRange(-1.0, 1.0);
	steer_pid.SetOutputRange(-1.0, 1.0);
	
	
	left_front_pid.Enable();
	right_front_pid.Enable();
	left_back_pid.Enable();
	right_back_pid.Enable();
	steer_pid.Enable();
	
	
	// by default, no traction control from accelerometer (because it doesn't work yet)
	accelerometer_enabled = false;

	// initialize accelerometers
	acc_x.SetZero(ACC_ZERO);
	acc_y.SetZero(ACC_ZERO);
	
	acc_x.SetSensitivity(ACC_SENSITIVITY);
	acc_y.SetSensitivity(ACC_SENSITIVITY);
	steer_pid.SetSetpoint(steer_abs_encoder.PIDGet());

	
	
}


/*
 * 
 * IO functions:
 * 
 * IO() -- handles joystick button presses, etc
 * AccelLog(FILE *f) -- debug output of accelerometers to a file
 * GetPot()	-- old function to get the pot value
 * 
 */


// communicate with the joystick
// current functionality:
// reset encoders and PID
// calibrate acceleromoeter (find zero point)
// disable traction control
// start attack mode
void RMRDrive::IO()
{
	static int i = 0;
	int period = DEBUG_OUT_PERIOD;
//	float zero;
	
	// history of button presses, to have something happen only when we release the button
	static bool disabled_traction_control = false;
	static bool started_attack_mode = false;
	
	
	// button to reset encoders and PID
	if (steering_wheel.GetRawButton(PID_RESET_BUTTON)) {
		printf("PID reset button was pressed\n");
		ResetEncoders();
		left_front_pid.Reset();
		left_front_pid.Enable();
		right_front_pid.Reset();
		right_front_pid.Enable();
		left_back_pid.Reset();
		left_back_pid.Enable();
		right_back_pid.Reset();
		right_back_pid.Enable();
		steer_pid.Reset();
		steer_pid.Enable();
		acc_x.ResetSpeed();
		acc_y.ResetSpeed();
	}
	
	// button to start the calibration
	if (steering_wheel.GetRawButton(ACCEL_CALIBRATE_BUTTON))
	{
		printf("accelerometer calibrate button was pressed\n");
		acc_x.Calibrate(m_watchdog);
		acc_y.Calibrate(m_watchdog);
	}
	
	if (steering_wheel.GetRawButton(STEERING_CALIBRATE_BUTTON))
	{
		printf("starting the calibration...\n");
		CalibrateSteering();
	}
	
	// button to disable traction control
	// releasing it enables traction control
	if (steering_wheel.GetRawButton(TRACTION_DISABLE))
	{
		printf("traction disabled\n");
		DisableTractionControl();
		disabled_traction_control = true;
	}
	else
	{
		if (disabled_traction_control == true)
		{
			printf("traction enabled\n");
			EnableTractionControl();
			disabled_traction_control = false;
		}
	}
	
	// button to start attack mode
	// disable PID, traction control, then put full power to the motors
	if (steering_wheel.GetRawButton(ATTACK_MODE))
	{
		if (started_attack_mode == false)
		{
			//printf("starting attack mode\n");
			DisableTractionControl();
			DisableDrivePIDs();
			started_attack_mode = true;
		}
		//printf("in attack mode\n");
		if (steering_wheel.GetRawButton(FRONT_BRAKE_BUTTON))
		{
			left_front_motor.Set(0);
			right_front_motor.Set(0);
			left_back_motor.Set(1.0);
			right_back_motor.Set(1.0);
		}
		else if(steering_wheel.GetRawButton(BACK_BRAKE_BUTTON))
		{
			left_front_motor.Set(1.0);
			right_front_motor.Set(1.0);
			left_back_motor.Set(0);
			right_back_motor.Set(0);
		}
		else
		{
			left_front_motor.Set(1.0);
			right_front_motor.Set(1.0);
			left_back_motor.Set(1.0);
			right_back_motor.Set(1.0);
		}
	}
	else
	{
		if (started_attack_mode == true)
		{
			//printf("exiting attack mode\n");
			left_front_motor.Set(0);
			right_front_motor.Set(0);
			left_back_motor.Set(0);
			right_back_motor.Set(0);
			//Traction control doesn't work
			DisableTractionControl();
			EnablePIDs();
			started_attack_mode = false;
		}
	}

//	zero = steering_wheel.GetZ() * 2;
//	acc_x.SetZero(zero);
	
	i++;
	if (i > period)
	{
		i = 0;
//		printf("limit switch left = %s, right = %s\n",
//				m_left_limit_switch.Get() ? "true" : "false",
//				m_right_limit_switch.Get() ? "true" : "false");
//		printf("setting zero = %f\n", zero);
	}
	
}

// log the current accelerometer x anc y voltages to a file
void RMRDrive::AccelLog(FILE* f)
{
	fprintf(f, "x: %8.4f, y: %8.4f\n", acc_x.GetVoltage(), acc_y.GetVoltage());
}

//Calibrate the left and right steering positions from limit switches
void RMRDrive::CalibrateSteering(void)
{
	printf("calibrating steering\n");
	steer_pid.Disable();
	steer_abs_encoder.AutoCalibrate(m_left_limit_switch, m_right_limit_switch, steer_motor, m_watchdog);
	steer_pid.Enable();
	printf("going to center\n");
//	printf("Setpoint: %f\n", steer_pid.GetSetpoint());
	steer_pid.SetSetpoint(0);
	IS_CAL = true;
	printf ("now is calibrated\n");
}






/*
 * Control functions:IS_CAL
 * 
 * ResetEncoders() -- reset the encoders to zero distance
 * KillAdvancedControls() -- stop all the fancy driving stuff
 * DisableTractionControl() -- turn off acceleration limits
 * EnableTractionControl() -- turn on acceleration limits
 * DisablePIDs() -- disable all motor PID controls
 * EnablePIDs() -- enable all motor PID controls
 */


// stop the pids, encoders, acceleration limits
void RMRDrive::KillAdvancedControls()
{
	printf("killing advanced controls\n");
	DisableDrivePIDs();

	left_front_encoder.Stop();
	right_front_encoder.Stop();
	left_back_encoder.Stop();
	right_back_encoder.Stop();

	DisableTractionControl();

}

// turn off acceleration limits in the wheels and the accelerometer 
void RMRDrive::DisableTractionControl()
{
	printf("disabling traction control\n");
	left_front_motor.DisableLimitAcceleration();
	right_front_motor.DisableLimitAcceleration();
	left_back_motor.DisableLimitAcceleration();
	right_back_motor.DisableLimitAcceleration();
	steer_motor.DisableLimitAcceleration();
	
	accelerometer_enabled = false;
}

// turn on acceleration limits in the wheels and the accelerometer 
void RMRDrive::EnableTractionControl()
{
	printf("enabling traction control\n");
	left_front_motor.EnableLimitAcceleration();
	right_front_motor.EnableLimitAcceleration();
	left_back_motor.EnableLimitAcceleration();
	right_back_motor.EnableLimitAcceleration();
	steer_motor.EnableLimitAcceleration();
	
	accelerometer_enabled = true;
}

// disable all drive motor PID control (doesn't affect steering)
void RMRDrive::DisableDrivePIDs()
{
	printf("disabling PIDs\n");
	left_front_pid.Disable();
	right_front_pid.Disable();
	left_back_pid.Disable();
	right_back_pid.Disable();
}

// Enable all motor PID control, including steering
void RMRDrive::EnablePIDs()
{
	printf("enabling PIDs\n");
	left_front_pid.Enable();
	right_front_pid.Enable();
	left_back_pid.Enable();
	right_back_pid.Enable();
	steer_pid.Enable();
}


// reset the 4 drive encoder distances to zero
void RMRDrive::ResetEncoders(void)
{
	printf("resetting encoders\n");
	left_front_encoder.Reset();
	right_front_encoder.Reset();
	left_back_encoder.Reset();
	right_back_encoder.Reset();
}






/*
 * Diagnostic tools:
 * 
 * Call these methods once each loop to get diagnostic tools
 * 
 * ShowEncoderDiagnostics() -- print out encoder position and speed info
 * TestJoyButtons() -- print out which steering_wheel button is pressed
 * TestMotors() -- cycle through all the motors, applying power to each in turn
 */

// print out encoder position and speed info
void RMRDrive::ShowEncoderDiagnostics()
{
	static int i = 0;
	int period = DEBUG_OUT_PERIOD;
	static bool first_time = true;
	
	
	// print out big first-time diagnostic messages
	// (formatted with extra-fancy tables!)
	if (first_time)
	{
		// stop all motors
		right_front_motor.Set(0);
		left_front_motor.Set(0);
		right_back_motor.Set(0);
		left_back_motor.Set(0);
		
		DisableDrivePIDs();
		
		printf("starting the encoder diagnostic tool\n");
		printf("turn the motors by hand, and I will tell you how the encoders move\n\n");
		
		printf("Configuration of the four encoders (FL, FR, BL, BR):\n");
		printf("%4s %6s %9s %6s %9s %8s %9s %8s %10s\n",
				"Name", "A Slot",
				"A Channel",
				"B Slot",
				"B Channel",
				"Scaling",
				"reversed?",
				"enabled?",
				"Speed Mode");
		
		// example non-computed
		//printf("%4s %6d %9d %6d %9d %7f %9s %8s %10s\n", "FL", 4, 1, 4, 1, 18.472, "yes", "no", "position");
		
		// note: the Scaling column has a width of 8, so the appropriate formats ar %8d, %8s, %7f
		// %7f is different because the decimal point isn't included in the width
		
		// steer encoder
		printf("%4s %6d %9d %6d %9d %8d %9s %8s %10s\n",
				"ST",
				SIDECAR_1,
				STEER_ENCODER,
				SIDECAR_1,
				STEER_ENCODER,
				steer_abs_encoder.GetOffset(),
				steer_abs_encoder.IsBackwards() ? "no" : "yes",
				"yes",
				"position");
		
		// front left encoder
		printf("%4s %6d %9d %6d %9d %7f %9s %8s %10s\n",
				"FL",
				SIDECAR_2,
				LEFT_FRONT_ENCODER_A,
				SIDECAR_2,
		        LEFT_FRONT_ENCODER_B,
            	left_front_encoder.GetScaling(),
                left_front_encoder.IsReversed() ? "yes" : "no",
				left_front_encoder.IsEnabled() ? "yes" : "no",
				left_front_encoder.IsSpeedMode() ? "speed" : "position");
		
		// front right encoder
		printf("%4s %6d %9d %6d %9d %7f %9s %8s %10s\n",
				"FR",
				SIDECAR_2,
				RIGHT_FRONT_ENCODER_A,
				SIDECAR_2,
		        RIGHT_FRONT_ENCODER_B,
            	right_front_encoder.GetScaling(),
                right_front_encoder.IsReversed() ? "yes" : "no",
				right_front_encoder.IsEnabled() ? "yes" : "no",
				right_front_encoder.IsSpeedMode() ? "speed" : "position");
		
		// back left encoder
		printf("%4s %6d %9d %6d %9d %7f %9s %8s %10s\n",
				"BL",
			    SIDECAR_2,
		        LEFT_BACK_ENCODER_A,
	            SIDECAR_2,
                LEFT_BACK_ENCODER_B,
                left_back_encoder.GetScaling(),
                left_back_encoder.IsReversed() ? "yes" : "no",
                left_back_encoder.IsEnabled() ? "yes" : "no",
                left_back_encoder.IsSpeedMode() ? "speed" : "position");
		
		// back right encoder
		printf("%4s %6d %9d %6d %9d %7f %9s %8s %10s\n",
				"BR",
				SIDECAR_2,
				RIGHT_BACK_ENCODER_A,
	            SIDECAR_2,
            	RIGHT_BACK_ENCODER_B,
                right_back_encoder.GetScaling(),
            	right_back_encoder.IsReversed() ? "yes" : "no",
        		right_back_encoder.IsEnabled() ? "yes" : "no",
    			right_back_encoder.IsSpeedMode() ? "speed" : "position");
		
		printf("\n");
		
		// example:
		// Name, A Slot, A Channel, B Slot, B Channel, scaling, reversed?, enabled?, Speed mode
		// FL    4       1			4		1		   18.472   yes		   yes		 speed
		// RL    4       1			4		1		   18.472   yes		   yes       position
		// BL    4       1			4		1		   18.472   yes		   yes       position
		// BR    4       1			4		1		   18.472   yes		   yes       position
		
		first_time = false;
	}
	
	
	
	//delayed debug output
	i++;
	if (i > period) 
	{
		i = 0;
//		
		

		printf("raw      FL = % 8d, FR = % 8d, BL = % 8d, BR = % 8d\n",
				left_front_encoder.Get(), right_front_encoder.Get(),
				left_back_encoder.Get(),  right_back_encoder.Get());
		
		printf("position FL = % 8.4f, FR = % 8.4f, BL = % 8.4f, BR = % 8.4f\n",
				left_front_encoder.GetPosition(), right_front_encoder.GetPosition(),
				left_back_encoder.GetPosition(),  right_back_encoder.GetPosition());
		
		printf("speed    FL = % 8.4f, FR = % 8.4f, BL = % 8.4f, BR = % 8.4f\n",
				left_front_encoder.GetSpeed(), right_front_encoder.GetSpeed(),
				left_back_encoder.GetSpeed(),  right_back_encoder.GetSpeed());

		
//		printf("direction FL = %s, FR = %s, BL = %s, BR = %s\n",
//				left_front_encoder.GetDirection() ? "yes" : "no", right_front_encoder.GetDirection() ? "yes" : "no",
//				left_back_encoder.GetDirection() ? "yes" : "no",  right_back_encoder.GetDirection() ? "yes" : "no");
	}
}

// print out which joystick button is pressed
void RMRDrive::TestJoyButtons(Joystick &joystick)
{
	static bool first_time = true;
	
	if (first_time)
	{
		first_time = false;
		printf("starting the test joy buttons\n");
		printf("press buttons on the joystick, and I will tell you which one you pressed\n");
	}
	
	UINT32 i;
	for (i = 1; i < 16; i++)
	{
		if (joystick.GetRawButton(i))
		{
			printf("Button %u\n", i);
		}
	}
}

// cycle through all the motors, applying power to each in turn
// do not use PID with this
void RMRDrive::TestMotors()
{
	static int count = 0;
	static bool first_time = true;
	
	int period = 100000;
	float speed;
	
	bool is_stopped;
	bool is_flipped;
	
	//DisablePIDs();


	count++;
	
	
	// print out stuff on first run - after this, the loop will automatically print stuff in the right order
	if (first_time)
	{
		first_time = false;
		
		printf("starting the motor diagnostics\n");
		printf("this will cycle through each motor in turn, driving each forward and reverse\n");
		
		// stop all motors
		right_front_motor.Set(0);
		left_front_motor.Set(0);
		right_back_motor.Set(0);
		left_back_motor.Set(0);
		
		DisableDrivePIDs();
		
		
		printf("\nMotor Configuration:\n");
		
		printf("%4s %6s %7s %9s\n",
				"Name",
				"Slot",
				"Channel",
				"reversed?");
		
		// left front motor
		printf("%4s %6d %7d %9s\n",
				"FL",
				SIDECAR_1,
				LEFT_FRONT_MOTOR_CHANNEL,
				left_front_encoder.GetDirection() ? "yes" : "no");

		// right front motor
		printf("%4s %6d %7d %9s\n",
				"FR",
				SIDECAR_1,
				RIGHT_FRONT_MOTOR_CHANNEL,
				right_front_encoder.GetDirection() ? "yes" : "no");
		
		// left back motor
		printf("%4s %6d %7d %9s\n",
				"BL",
				SIDECAR_1,
				LEFT_BACK_MOTOR_CHANNEL,
				left_back_encoder.GetDirection() ? "yes" : "no");
		
		// right back motor
		printf("%4s %6d %7d %9s\n",
				"BR",
				SIDECAR_1,
				RIGHT_BACK_MOTOR_CHANNEL,
				right_back_encoder.GetDirection() ? "yes" : "no");
		
		printf("\n");
		

		
		printf("driving the FR motor forwards...\n");
	}
	
	if (count < period * 1)
	{
		// turn on the FR motor, full forward
		right_front_motor.Set(1);
	}
	else if (count == period * 1)
	{
		// check the encoder speed for the FR motor
		speed = right_front_encoder.GetSpeed();
		is_stopped = (speed == 0);
		is_flipped = (speed < 0);
		printf("the encoder measured a speed of %f rev/sec on the FR motor %s\n",
				speed, is_stopped ? "STOPPED" : is_flipped ? "FLIPPED" : "");
		printf("driving the FR motor backwards...\n");
	}
	else if (count < period * 2)
	{
		// turn on the FR motor, full backwards
		right_front_motor.Set(-1);
	}
	else if (count == period * 2)
	{
		// check the encoder speed for the FR motor
		speed = right_front_encoder.GetSpeed();
		is_stopped = (speed == 0);
		is_flipped = (speed > 0);
		printf("the encoder measured a speed of %f rev/sec on the FR motor %s\n",
				speed, is_stopped ? "STOPPED" : is_flipped ? "FLIPPED" : "");
		printf("driving the FL motor forwards...\n");
	}
	else if (count < period * 3)
	{
		// turn on the FL motor, full forward
		right_front_motor.Set(0);
		left_front_motor.Set(1);
	}
	else if (count == period * 3)
	{
		// check the encoder speed for the FL motor
		speed = left_front_encoder.GetSpeed();
		is_stopped = (speed == 0);
		is_flipped = (speed < 0);
		printf("the encoder measured a speed of %f rev/sec on the FL motor %s\n",
				speed, is_stopped ? "STOPPED" : is_flipped ? "FLIPPED" : "");
		printf("driving the FL motor backwards...\n");
	}
	else if (count < period * 4)
	{
		// turn on the FL motor, full backwards
		left_front_motor.Set(-1);
	}
	else if (count == period * 4)
	{
		// check the encoder speed for the FL motor
		speed = left_front_encoder.GetSpeed();
		is_stopped = (speed == 0);
		is_flipped = (speed > 0);
		printf("the encoder measured a speed of %f rev/sec on the FL motor %s\n",
				speed, is_stopped ? "STOPPED" : is_flipped ? "FLIPPED" : "");
		printf("driving the BR motor forwards...\n");
	}
	else if (count < period * 5)
	{
		// turn on the BR motor, full forwards
		left_front_motor.Set(0);
		right_back_motor.Set(1);
	}
	else if (count == period * 5)
	{
		// check the encoder speed for the BR motor
		speed = right_back_encoder.GetSpeed();
		is_stopped = (speed == 0);
		is_flipped = (speed < 0);
		printf("the encoder measured a speed of %f rev/sec on the BR motor %s\n",
				speed, is_stopped ? "STOPPED" : is_flipped ? "FLIPPED" : "");
		printf("driving the BR motor backwards...\n");
	}
	else if (count < period * 6)
	{
		// turn on the BR motor, full backwards
		right_back_motor.Set(-1);
	}
	else if (count == period * 6)
	{
		// check the encoder speed for the BR motor
		speed = right_back_encoder.GetSpeed();
		is_stopped = (speed == 0);
		is_flipped = (speed > 0);
		printf("the encoder measured a speed of %f rev/sec on the BR motor %s\n",
				speed, is_stopped ? "STOPPED" : is_flipped ? "FLIPPED" : "");
		printf("driving the BL motor forwards...\n");
	}
	else if (count < period * 7)
	{
		// turn on the BL motor, full forwards
		right_back_motor.Set(0);
		left_back_motor.Set(1);
	}
	else if (count == period * 7)
	{
		// check the encoder speed for the BL motor
		speed = left_back_encoder.GetSpeed();
		is_stopped = (speed == 0);
		is_flipped = (speed < 0);
		printf("the encoder measured a speed of %f rev/sec on the BL motor %s\n",
				speed, is_stopped ? "STOPPED" : is_flipped ? "FLIPPED" : "");
		printf("driving the BL motor backwards...\n");
	}
	else if (count < period * 8)
	{
		// turn on the BL motor, full backwards
		left_back_motor.Set(-1);
	}
	else if (count == period * 8)
	{
		// check the encoder speed for the BL motor
		speed = left_back_encoder.GetSpeed();
		is_stopped = (speed == 0);
		is_flipped = (speed > 0);
		printf("the encoder measured a speed of %f rev/sec on the BL motor %s\n",
				speed, is_stopped ? "STOPPED" : is_flipped ? "FLIPPED" : "");
		printf("driving the FR motor forwards...\n");
	}
	else
	{
		left_back_motor.Set(0);
		// reset count
		count = 0;
	}
	

}




/*
 * Driving routines:
 * 
 * TankDrive2 				(basic tank drive with PID, controlled by two joysticks -- we uses this to test PID on the kitbot)
 * CarDrive 				(main drive function, controls the robot via CarDrive from a steering_wheel)
 * CarDrive(speed, turn)	(do the math for CarDrive)
 * 
 * SetPosition				(old, pre PID steering routine)
 * 
 */

// PID controlled tank drive
// two joysticks set the left and right setpoints based on Y axis
void RMRDrive::Tank_Drive2()
{
	static int i = 0;
	int period = DEBUG_OUT_PERIOD;
	
	/*
	 * GetSpeed if feet/sec   * 12 in / 1 ft * 1 rev / INCHES_PER_REV in = rev / sec
	 */
	
//	float acc_speed = acc_y.GetSpeed() * 12.0 / INCHES_PER_REV;

	// compute the left and right setpoints (desired speeds) from the joystick
	// the ScaleRangeToRange scales the -1..1 joystick axis to the MIN_SPEED..MAX_SPEED range
	//    ie full forward is MAX_SPEED, full reverse is MIN_SPEED
	float left_setpoint = -1 * ScaleRangeToRange(steering_wheel.GetY(), -1, 1, MIN_SPEED, MAX_SPEED);
	float right_setpoint = -1 * ScaleRangeToRange(joystick2.GetY(), -1, 1, MIN_SPEED, MAX_SPEED);
	
	// debug stuff... print these lines once every period loops
	i++;
	if (i > period) {
		i = 0;
		printf("setpoint left = %8.4f, right = %8.4f\n", left_front_pid.GetSetpoint(), right_front_pid.GetSetpoint());
//		printf("speed    left = %8.4f, right = %8.4f ", left_front_encoder.GetSpeed(), right_front_encoder.GetSpeed());
//		printf("Acc Speed yaxis:%8.4f accel ga:%8.4f\n", acc_speed, acc_y.GetFilteredAccel());
	}
	
	// send the setpoints off to the PID
	steer_pid.SetSetpoint(0);
	left_front_pid.SetSetpoint(left_setpoint);
	right_front_pid.SetSetpoint(right_setpoint);
	left_back_pid.SetSetpoint(left_setpoint);
	right_back_pid.SetSetpoint(right_setpoint);
}



// car drive the robot from 1 joystick: Y axis is speed, X axis is turn
void RMRDrive::Car_Drive()
{
	float speed, turn;

	
	speed = -1 * ScaleRangeToRange(steering_wheel.GetY(), -1.0, 1.0, MIN_SPEED, MAX_SPEED);
	turn =       ScaleRangeToRange(steering_wheel.GetX(), -1.0, 1.0, MIN_TURN, MAX_TURN);
	// flip the y-axis
	
	if (CAR_DRIVE_INDIVIDUAL)
		Car_Drive_Individual(speed, turn);
	else
		Car_Drive(speed, turn);
	
}

// turn at a desired speed and turn, controlling all 4 motors individually
void RMRDrive::Car_Drive_Individual(float speed, float turn)
{
	static int i = 0;
	int period = DEBUG_OUT_PERIOD;
	
	float left_front_out, right_front_out;
	float left_back_out, right_back_out;
	
	float left_front_radius, right_front_radius;
	float left_back_radius, right_back_radius;
	
	float left_front_accel_speed, right_front_accel_speed;
	float left_back_accel_speed, right_back_accel_speed;
	
	float accel_speed = Hypot(acc_y.GetSpeed(), acc_x.GetSpeed()) * 12.0 / INCHES_PER_REV;
	
	float center_radius;
	float accel_radius;
	
	// the ScaleRangeToRange(...) is in terms of -PI/2 left, 0 straight, PI/2 right
	// the fabs(...) is PI/2 on either side, 0 straight
	// the PI/2 - (...) is 0 on either side, PI/2 straight
	float theta = PI/2 - fabs(ScaleRangeToRange(turn, -1.0, 1.0, -PI/2, PI/2));
	
	
	// if we are turning, calculate the turning radius
	if (turn < 0) // turning left
	{
		left_back_radius   = WHEEL_BASE * tan(theta);
		left_front_radius  = Hypot(WHEEL_BASE, left_back_radius);
		right_back_radius  = left_back_radius + WHEEL_TRACK;
		right_front_radius = Hypot(WHEEL_BASE, right_back_radius);
		
		center_radius = Hypot(left_back_radius + WHEEL_TRACK/2, WHEEL_BASE/2);	
		accel_radius =  Hypot(left_back_radius + WHEEL_TRACK/2, ACCEL_DISTANCE_FROM_BACK);
	}
	else if (turn > 0) // turning right
	{
		right_back_radius  = WHEEL_BASE * tan(theta);
		right_front_radius = Hypot(WHEEL_BASE, right_back_radius);
		left_back_radius   = right_back_radius + WHEEL_TRACK;
		left_front_radius  = Hypot(WHEEL_BASE, left_back_radius);
		
		center_radius = Hypot(right_back_radius + WHEEL_TRACK/2, WHEEL_BASE/2);
		accel_radius =  Hypot(right_back_radius + WHEEL_TRACK/2, ACCEL_DISTANCE_FROM_BACK);
	}
	
	
	// calculate left_front_out, etc, left_front_accel_speed, etc
	if (turn == 0) // drive straight
	{
		left_front_out  = speed;
		right_front_out = speed;
		left_back_out   = speed;
		right_back_out  = speed;
		
		left_front_accel_speed  = accel_speed;
		right_front_accel_speed = accel_speed;
		left_back_accel_speed   = accel_speed;
		right_back_accel_speed  = accel_speed;
	}
	else // turn
	{
		left_front_out  = left_front_radius  / center_radius * speed;
		right_front_out = right_front_radius / center_radius * speed;
		left_back_out   = left_back_radius   / center_radius * speed;
		right_back_out  = right_back_radius  / center_radius * speed;
		
		
		left_front_accel_speed  = left_front_radius  / accel_radius * accel_speed;
		right_front_accel_speed = right_front_radius / accel_radius * accel_speed;
		left_back_accel_speed   = left_back_radius   / accel_radius * accel_speed;
		right_back_accel_speed  = right_back_radius  / accel_radius * accel_speed;
		
	}

	
	// AntiSlip calculations (limit wheel output to prevent slipping)
	if (ANTI_SLIP_ENABLED)
	{
		left_front_out  = AntiSlipWheel(left_front_encoder.GetSpeed(),  left_front_accel_speed,  left_front_out);
		right_front_out = AntiSlipWheel(right_front_encoder.GetSpeed(), right_front_accel_speed, right_front_out);
		left_back_out   = AntiSlipWheel(left_back_encoder.GetSpeed(),   left_back_accel_speed,   left_back_out);
		right_back_out  = AntiSlipWheel(right_back_encoder.GetSpeed(),  right_back_accel_speed,  right_back_out);
	}
	
	
#if !BRAKE_ENABLED	
	// send the desired speeds off to the PID
	left_front_pid.SetSetpoint(left_front_out);
	right_front_pid.SetSetpoint(right_front_out);
	left_back_pid.SetSetpoint(left_back_out);
	right_back_pid.SetSetpoint(right_back_out);
#else	
	if (steering_wheel.GetRawButton(FRONT_BRAKE_BUTTON))
		{
			printf("Front Break Enabled \n");
			left_front_pid.SetSetpoint(0);
			right_front_pid.SetSetpoint(0);
			left_back_pid.SetSetpoint(left_back_out);
			right_back_pid.SetSetpoint(right_back_out);
		}
		else if(steering_wheel.GetRawButton(BACK_BRAKE_BUTTON))
		{
			printf("Back Break Enabled \n");
			left_front_pid.SetSetpoint(left_front_out);
			right_front_pid.SetSetpoint(right_front_out);
			left_back_pid.SetSetpoint(0);
			right_back_pid.SetSetpoint(0);
		}
		else
		{
			left_front_pid.SetSetpoint(left_front_out);
			right_front_pid.SetSetpoint(right_front_out);
			left_back_pid.SetSetpoint(left_back_out);
			right_back_pid.SetSetpoint(right_back_out);
		}
#endif	
	
	// steer
	steer_pid.SetSetpoint(turn);
	
	i++;
	if (i > period)
	{
		i = 0;
		printf("starting individual speed control: speed = %8.4f, turn = %8.4f\n", speed, turn);
		printf("radius: LF, RF, LB, RB, C = %8.4f, %8.4f, %8.4f, %8.4f, %8.4f\n",
				left_front_radius, right_front_radius, left_back_radius, right_back_radius, center_radius);
		printf("out:    LF, RF, LB, RB    = %8.4f, %8.4f, %8.4f, %8.4f\n",
				left_front_out, right_front_out, left_back_out, right_back_out);
	}
	
}

// limit wheel output to prevent slipping
float RMRDrive::AntiSlipWheel(float wheel_encoder_speed, float wheel_accel_speed, float wheel_desired_speed)
{
	static float ALLOWED_SLIP = 1.0;
	if (wheel_encoder_speed - wheel_accel_speed > ALLOWED_SLIP)
	{
		return wheel_accel_speed + ALLOWED_SLIP;
	}
	else if (wheel_encoder_speed - wheel_accel_speed < -ALLOWED_SLIP)
	{
		return wheel_accel_speed - ALLOWED_SLIP;
	}
	else
	{
		return wheel_desired_speed;
	}
}

// this function drives the motors to travel at the desired speed  at the desired heading
// speed is in the range MIN_SPEED to MAX_SPEED and turn is in the range -1 .. 1
void RMRDrive::Car_Drive(float speed, float turn)
{
	static int i = 0;
	static int period = DEBUG_OUT_PERIOD;
	
	float l_drive_motor_out, r_drive_motor_out;
	
	//float acc_speed = ScaleRangeToRange(sqrt((acc_y.GetSpeed() * acc_y.GetSpeed()) + (acc_x.GetSpeed() * acc_x.GetSpeed())), -1.0, 1.0, MIN_SPEED, MAX_SPEED); 
//	float acc_speed = acc_y.GetSpeed();
	float front_speed = speed;
	
	// put these values in the range MIN_SPEED .. MAX_SPEED
	speed = Limit(speed, MIN_SPEED, MAX_SPEED);
	turn = Limit(turn, -1.0, 1.0);
	front_speed = speed;
	
	l_drive_motor_out = speed; 
	r_drive_motor_out = speed;
	
	if (ENABLE_TURN_SLOWDOWN)
	{
		l_drive_motor_out += turn * TURNING_SIDE_SLOWDOWN_FACTOR;
		r_drive_motor_out -= turn * TURNING_SIDE_SLOWDOWN_FACTOR;
	}

	
//	left_front_motor.PIDWrite(l_drive_motor_out);
//	left_back_motor.Set(l_drive_motor_out);
//	right_front_motor.Set(r_drive_motor_out);
//	right_back_motor.Set(r_drive_motor_out);

#if 1
	if (steering_wheel.GetRawButton(FRONT_BRAKE_BUTTON))
	{
		printf("Front Break Enabled \n");
		left_front_pid.SetSetpoint(0);
		right_front_pid.SetSetpoint(0);
		left_back_pid.SetSetpoint(l_drive_motor_out);
		right_back_pid.SetSetpoint(r_drive_motor_out);
	}
	else if(steering_wheel.GetRawButton(BACK_BRAKE_BUTTON))
	{
		printf("Back Break Enabled \n");
		left_front_pid.SetSetpoint(front_speed);
		right_front_pid.SetSetpoint(front_speed);
		left_back_pid.SetSetpoint(0);
		right_back_pid.SetSetpoint(0);
	}
	else
	{
		left_front_pid.SetSetpoint(front_speed);
		right_front_pid.SetSetpoint(front_speed);
		left_back_pid.SetSetpoint(l_drive_motor_out);
		right_back_pid.SetSetpoint(r_drive_motor_out);
	}
#else	

	left_front_motor.Set(l_drive_motor_out);
	right_front_motor.Set(r_drive_motor_out);
	left_back_motor.Set(l_drive_motor_out);
	right_back_motor.Set(r_drive_motor_out);
#endif	
	
	// Old Code....
//	
//	// use this to steer by pot
//	potv = (float) pot.GetAverageValue();
//	current_steer_position = POT_DIRECTION
//		* ScaleRangeToRange(potv, POT_MIN, POT_MAX, -1.0, 1.0);
//
//	
//	// use this to steer by digital absolute encoder
//	anglev = (float) 1.0e6*abs_encoder_count.GetPeriod();
//	current_steer_position = ANGLE_DIRECTION
//		* ScaleRangeToRange(anglev, ANGLE_MIN, ANGLE_MAX, -1.0, 1.0);
//	
//	SetPosition(steer_motor, turn, current_steer_position);

	steer_pid.SetSetpoint(turn);
	
	// debug stuff... print these lines once every period loops
	i++;
	if (i > period) {
		i = 0;
//		printf("raw left front = %d, raw right front = %d\n", left_front_encoder.Get(), right_front_encoder.Get());
//		printf("rout: %f lout: %f\n", r_drive_motor_out, l_drive_motor_out);
//		printf("target speed = %f, target turn = %f\n", speed, turn);
//		printf("potv = %.0f, scaled to %f\n", potv, current_steer_position);
//		printf("anglev = %.0f, scaled to %f\n", anglev, current_steer_position);
////		printf("left encoder is %5i right encoder is %5i\n", left_encoder.Get(), right_encoder.Get());
//		printf("Position: FL=%8.2f FR=%8.2f BL=%8.4f BR=%8.4f\n",
//				left_front_encoder.GetPosition(), right_front_encoder.GetPosition(),
//				left_back_encoder.GetPosition(),  right_back_encoder.GetPosition());
//		printf("Speed:    FL=%8.2f FR=%8.2f BL=%8.4f BR=%8.4f\n",
//				left_front_encoder.GetSpeed(), right_front_encoder.GetSpeed(),
//				left_back_encoder.GetSpeed(),  right_back_encoder.GetSpeed());
//		printf("Encoder       Speed: Left=%8.2f Right=%8.2f\n", left_encoder.GetSpeed() * FEET_PER_REV, right_encoder.GetSpeed() * FEET_PER_REV);
//		printf("clock = %f", GetClock());
//		printf("Accel        x=%8.2f     y=%8.2f\n", acc_x.GetFilteredAccel(), acc_y.GetFilteredAccel());
//		printf("Accelerometer Speed     x=%8.2f     y=%8.2f\n", acc_x.GetSpeed(), acc_y.GetSpeed());
//		printf("PID Set = %8.2f, Error = %8.2f, Out = %8.2f\n", left_front_pid.GetSetpoint(), left_front_pid.GetError(), left_front_pid.Get());
//		printf("y-axis is%f\n", steering_wheel.GetY());
//		printf("y-axis is %f, limited speed is %f\n", steering_wheel.GetY(), speed);
		printf("setpoint speed = %8.4f, setpoint turn = %8.4f, current turn = %8.4f\n", speed, turn, steer_abs_encoder.PIDGet());
//		printf("calibrated:%d\n", IS_CAL);
		ToggleRIOUserLED();
	}
	
}



// old (pre-PID) function to steer motor by pot
void RMRDrive::SetPosition(Jaguar &motor, float desired_position, float current_position)
{
	static int i = 0;
	int period = DEBUG_OUT_PERIOD;
	
	
	float err;
	
	// make the variables be in the range -1 .. 1
	desired_position = Limit(desired_position);
	current_position = Limit(current_position);
	
	err = desired_position - current_position;
	
	i++;
	if (i > period)
	{
		i = 0;
		//printf("motor is set to %f\n", err / 2.0);
	}
	
	motor.Set(err / 2.0);
	
	
}


/*
 * A_wanted = Limit(A_wanted, A_current - k, A_current + k)
 * 
 * 
 * if (A_encoder - A_current > k) // if we are slipping (wheels turn too fast)
 * {
 * 		A_wanted = A_current + k; // limit desired speed to a little bit more positive than current speed
 * }
 * elseif (A_encoder - A_current < -k) // if we are slipping (wheels turn too slow)
 * 		A_wanted = A_current - k; // limit desired speed to a little bit more negative than current speed
 * else
 * 		A_wanted = A_radius / Robot_center_radius * Robot_center_speed;
 * 
 * 
 * 
 * if (turn < 0) // turning left
 * {
 * 		
 * 
 * 
 */

